; BLOCK.LISP
; Icarus concepts, primitive skills, and goals for the Blocks World.

; Create a set of conceptual clauses for the Blocks World.
(create-concepts

; ON describes a situation in which one block is on top of another.
 ((on ?block1 ?block2)
  :percepts ((block ?block1 xpos ?xpos1 ypos ?ypos1)
	     (block ?block2 xpos ?xpos2 ypos ?ypos2 height ?height2))
  :tests ((equal ?xpos1 ?xpos2) (= ?ypos1 (+ ?ypos2 ?height2))))

; ONTABLE describes a situation in which a block in sitting on the table.
 ((ontable ?block1 ?table)
  :percepts ((block ?block1 xpos ?xpos1 ypos ?ypos1)
	     (table ?table xpos ?xpos2 ypos ?ypos2 width ?width2 height ?height2))
  :tests ((<= ?xpos2 ?xpos1) (>= (+ ?xpos2 ?width2) ?xpos1) (>= ?ypos1 ?ypos2) (<= ?ypos1 (+ ?ypos2 ?height2))))


; CLEAR matches when a block has no other block on top of it.
 ((clear ?block)
  :percepts ((block ?block))
  :relations ((not (on ?other-block ?block))))

; HOLDING is satisfied when the hand is holding a block.
 ((holding ?block)
  :percepts ((hand ?hand status ?block) (block ?block)))

; HAND-EMPTY describes a situation in which the hand is not holding a block.
 ((hand-empty)
  :percepts ((hand ?hand status ?status))
  :relations ((not (holding ?any)))
  :tests ((eq ?status 'empty)))

; THREE-TOWER matches when there exists a tower composed of three blocks.
 ((three-tower ?x ?y ?z ?table)
  :percepts ((block ?x) (block ?y) (block ?z) (table ?table))
  :relations ((on ?x ?y) (on ?y ?z) (ontable ?z ?table)))

; UNSTACKABLE is satisfied when it is possible to unstack one block from another.
 ((unstackable ?block ?from)
  :percepts ((block ?block) (block ?from))
  :relations ((on ?block ?from) (clear ?block) (hand-empty)))

; PICKUPABLE matches when it is possible to pick a block up from the table.
 ((pickupable ?block ?from)
  :percepts ((block ?block) (table ?from))
  :relations ((ontable ?block ?from) (clear ?block) (hand-empty)))

; STACKABLE describes a situation in which a block can be stacked on another.
 ((stackable ?block ?to)
  :percepts ((block ?block) (block ?to))
  :relations ((clear ?to) (holding ?block)))

; PUTDOWNABLE is satisfied when it is possible to put a held block on the table.
 ((putdownable ?block ?to)
  :percepts ((block ?block) (table ?to))
  :relations ((holding ?block)))

; UNSTACKED matches when the agent is holding one block that is not on another.
 ((unstacked ?block ?from)
  :percepts ((block ?block) (block ?from))
  :relations ((holding ?block) (not (on ?block ?from))))

; PICKED-UP matches when the agent is holding one block that is not on the table.
 ((picked-up ?block ?from)
  :percepts ((block ?block) (table ?from))
  :relations ((holding ?block) (not (ontable ?block ?from))))

; STACKED matches when the agent is not holding one block that is on another.
 ((stacked ?block ?to)
  :percepts ((block ?block) (block ?to))
  :relations ((on ?block ?to) (not (holding ?block))))

; PUT-DOWN matches when the agent is not holding a block that is on the table.
 ((put-down ?block ?to)
  :percepts ((block ?block) (table ?to))
  :relations ((ontable ?block ?to) (not (holding ?block))))
)

; Create a set of primitive skill clauses for the Blocks World.
(create-skills

; Achieve UNSTACKED by grasping and moving an unstackable block.
 ((unstacked ?block ?from)
  :percepts ((block ?block) (block ?from))
  :start ((unstackable ?block ?from))
  :actions ((*grasp ?block) (*vertical-move ?block 100)))

; Achieve PICKED-UP by grasping and moving a pickuable block.
 ((picked-up ?block ?from)
  :percepts ((block ?block) (table ?from))
  :start ((pickupable ?block ?from))
  :actions ((*grasp ?block) (*vertical-move ?block 100)))

; Achieve STACKED by moving and ungrasping a stackable block.
 ((stacked ?block ?to)
  :percepts ((block ?block) (block ?to xpos ?x ypos ?y  height ?h))
  :start ((stackable ?block ?to))
  :actions ((*horizontal-move ?block ?x) (*vertical-move ?block (+ ?y ?h)) (*ungrasp ?block)))

; Achieve PUT-DOWN by moving and ungrasping a putdownable block.
 ((put-down ?block ?to)
  :percepts ((block ?block width ?w) (table ?to xpos ?tx ypos ?ty) (marker 'FREE xpos ?x))
  :start ((putdownable ?block ?to))
  :actions ((*horizontal-move ?block ?x) (*vertical-move ?block ?ty) (*ungrasp ?block) (*update-free 'FREE (+ ?x ?w))))
)